#include "AgentsLanguageAux.h"
#include <stdlib.h>

namespace AgentsLanguage
{

	void  Aux::getMemory(unsigned char **p, int rX, int rY)
	{
		*p = (unsigned char *)malloc(rX*rY * 3 * sizeof(unsigned char));
	}


	double Aux::readVisionSensor(int clientID, int sensorHandler)
	{
		simxUChar *img = NULL;
		simxInt resolution[2] = { 1, 1 };
		getMemory(&img, resolution[0], resolution[1]);
		simxGetVisionSensorImage(clientID, sensorHandler, resolution, &img,
			0, simx_opmode_oneshot_wait);

		double pixelVal = (double)img[0];
		
		return pixelVal;
	}
	void Aux::setMotorTargetVelocity(int clientID, int motorHandler, double velocity)
	{
		simxSetJointTargetVelocity(clientID, motorHandler,
			(simxFloat)velocity, simx_opmode_oneshot);
	}

	double Aux::readProximitySensor(int clientID, int sensorHandler)
	{
		simxUChar sensorTrigger = 0;
		simxReadProximitySensor(clientID, sensorHandler, &sensorTrigger, NULL, NULL, NULL, simx_opmode_streaming);

		return sensorTrigger;
	}

}